
#include "vehicle-control-v1.h"

namespace vehicle_control_v1
{
	class VehicleControlVars : public SessionSampleVars
	{
	public:
		SPADAS_VARS(VehicleControl, SessionSampleVars)
		VehicleControlData data;

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp() = timestamp;

			Array<GeneralElement> values(6);
			sample.values() = values;

			values[0] = (Double)(Int)data.longitudinalMode;
			values[1] = data.longitudinalValue;
			values[2] = data.targetSpeed;
			values[3] = (Double)(Int)data.lateralMode;
			values[4] = data.lateralValue;
			values[5] = (Double)(Int)data.gearMode;

			sample.significantCount() = values.size();
			return sample;
		}

		Bool fromSample(String protocol, SessionSample sample) override
		{
			if (!sample.is<SessionGeneralSample>()) return FALSE;

			auto generalSample = sample.as<SessionGeneralSample>();
			timestamp = generalSample.timestamp();
			auto values = generalSample.values();

			if (protocol == "vehicle-control-v1")
			{
				if (values.size() != 6) return FALSE;

				data.longitudinalMode = values[0].valid ? (LongitudinalControlMode)(Int)values[0].value : LongitudinalControlMode::NoRequest;
				data.longitudinalValue = values[1].value;
				data.targetSpeed = values[2].value;
				data.lateralMode = values[3].valid ? (LateralControlMode)(Int)values[3].value : LateralControlMode::NoRequest;
				data.lateralValue = values[4].value;
				data.gearMode = values[5].valid ? (GearControlMode)(Int)values[5].value : GearControlMode::NoRequest;

				return TRUE;
			}

			return FALSE;
		}
	};
}

using namespace vehicle_control_v1;

VehicleControl::VehicleControl() : Object<VehicleControlVars>(new VehicleControlVars, TRUE)
{}

FullTimestamp& VehicleControl::timestamp()
{
	return vars->timestamp;
}

VehicleControlData *VehicleControl::operator ->()
{
    return &vars->data;
}

String VehicleControl::getProtocolName(UInt channel)
{
    return (String)"vehicle-control-v1@" + channel;
}

SessionGeneralSample VehicleControl::toGeneralSample()
{
    return vars->toGeneralSample();
}

Array<String> VehicleControl::getProtocolNames(UInt channel)
{
	return Array<String>::create(1, "vehicle-control-v1").convert<String>([channel](auto& s){ return s + "@" + channel; });
}

Bool VehicleControl::fromGeneralSample(String protocol, SessionGeneralSample generalSample)
{
    return vars->fromSample(protocol, generalSample.as<SessionSample>());
}